#include <UnitTest++.h>
#include <Math/Matrix.hpp>
#include <Math/Random.hpp>
#include <Algorithm/GramSchmidt.hpp>
#include <GraphicsAlgo/RotationMatrix.hpp>

using namespace zzz;

SUITE(RotationMatrixTest)
{
  TEST(GramSchmidtTest)
  {
    Matrix<8,8,double> mat,mat2;
    RandomReal<double> r;
    for (int i=0; i<64; i++)
      mat[i]=r.Rand();
    GramSchmidt<double>::Process(mat,mat2);
    for (int i=0; i<8; i++) for (int j=i; j<8; j++)
    {
      if (i==j)
        CHECK_CLOSE(1,FastDot(mat2.Row(i),mat2.Row(j)),EPSILON);
      else
        CHECK_CLOSE(0,FastDot(mat2.Row(i),mat2.Row(j)),EPSILON);
    }
  }

  TEST(QRDecompositionTest)
  {
    Matrix<8,8,double> mat;
    RandomReal<double> r;
    for (int i=0; i<64; i++)
      mat[i]=r.Rand();
    Matrix<8,8,double> Q,R;
    GramSchmidt<double>::QRDecomposition(mat,Q,R);
    CHECK(mat.DistTo(Q*R)<EPSILON);
  }

  template<int D> void CheckRotationMatrix(const Vector<D,double> &from, const Vector<D,double> &to)
  {
    Vector<D,double> to2(to);
    Matrix<D,D,double> rot=GetRotationBetweenVectors(from,to);
    to2=rot*from;
    for (zuint i=0; i<D; i++)
      CHECK_CLOSE(to[i],to2[i],EPSILON);
    for (zuint i=0; i<D; i++)
      CHECK_CLOSE(1.0,rot.Row(i).Len(),EPSILON);
    for (zuint i=0; i<D; i++) for (zuint j=0; j<i; j++)
      if (i==j) CHECK_CLOSE(1.0,FastDot(rot.Row(i),rot.Row(j)),EPSILON);
      else CHECK_CLOSE(0.0,FastDot(rot.Row(i),rot.Row(j)),EPSILON);

  }

  TEST(NDRotationMatrixTest)
  {
#define DIM (8)
    RandomReal<double> r;
    Vector<DIM,double> from,to,to2;
    for (zuint i=0; i<DIM; i++)
    {
      from[i]=r.Rand();
      to[i]=r.Rand();
    }
    from[0]=0;
    from.Normalize();
    to[0]=0;
    to[1]=0;
    to.Normalize();
    CheckRotationMatrix<DIM>(from,to);

    to=from;
    CheckRotationMatrix<DIM>(from,to);

    to=-from;
    CheckRotationMatrix<DIM>(from,to);
  }

  TEST(2DRotationMatrixTest)
  {
    Vector2d from(1,1),to(1,-1);
    from.Normalize();
    to.Normalize();
    CheckRotationMatrix<2>(from,to);
  }

  TEST(3DRotationMatrixTest)
  {
    Vector3d from(1,1,0),to(1,-1,0);
    from.Normalize();
    to.Normalize();
    CheckRotationMatrix<3>(from,to);
  }


}